/***************************************************************************
 创建者: 华磊
 开始时间: 2020.3.1
 copyright: (C) 华友高科
 ***************************************************************************/
#include "followmotion.h"
#include "axisobject.h"

FollowMotion::FollowMotion( double deltaTimeIn,MotionMessage *motionMessageIn)
{
    motionMessage=motionMessageIn;
    deltaTime=deltaTimeIn;
}

int FollowMotion::startFollow()
{
    currentFollowPoint=pointQueue.dequeue();
    return 1;
}

int FollowMotion::setFollowSpeed(double speedIn)
{
    followSpeed=speedIn;
    return 1;
}

int FollowMotion::setOrientationStrategy(int strategy)
{

}

int FollowMotion::setEndPoint(PointPro pointIn)
{
    endPoint=pointIn;
    return 1;
}

int FollowMotion::addDetectPoint(PointPro pointIn)
{
    pointQueue.enqueue(pointIn);
    return 1;;
}

int FollowMotion::reset()
{
    speedRatioAxis->resetAll();
    speedRatioAxis->positionCommand=0.0;
    speedRatioAxis->positionCommandOld=0.0;
    speedRatioAxis->positionFeedback=0.0;

    speedRatioAxis->clearTraj();//保证立刻覆盖前面的旧轨迹
    speedRatioAxis->splineZeroMoveByTime(1,2);
    pointQueue.clear();
}

int FollowMotion::getTrackPoint(PointPro &pointOut)
{//相同的点不要放进队列
    //
    //开始和结束有加速和减速.中间使用匀速插值.
    //使用姿态控制,保证tcp z轴在微小轨迹平面的投影与轨迹方向重合.如果为直线时,无法计算,因为没有微小平面.
    //曲率变化小于设定值时,姿态不变,保证稳定姿态,曲率变化大时,姿态插值变化.
    speedRatioAxis->moveOneStep();
    double targetLength=speedRatioAxis->positionCommand*followSpeed*deltaTime;
    PointPro tmpPoint=pointQueue.front();
    double segLength=sqrt((tmpPoint.positionValue[0]-currentFollowPoint.positionValue[0])*(tmpPoint.positionValue[0]-currentFollowPoint.positionValue[0])
            +(tmpPoint.positionValue[1]-currentFollowPoint.positionValue[1])*(tmpPoint.positionValue[1]-currentFollowPoint.positionValue[1])
            +(tmpPoint.positionValue[2]-currentFollowPoint.positionValue[2])*(tmpPoint.positionValue[2]-currentFollowPoint.positionValue[2]));
    pointOut.positionValue.resize(6);
    double moveRatio=targetLength/segLength;
    pointOut.positionValue[0]=currentFollowPoint.positionValue[0]+moveRatio*(tmpPoint.positionValue[0]-currentFollowPoint.positionValue[0]);
    pointOut.positionValue[1]=currentFollowPoint.positionValue[1]+moveRatio*(tmpPoint.positionValue[1]-currentFollowPoint.positionValue[1]);
    pointOut.positionValue[2]=currentFollowPoint.positionValue[2]+moveRatio*(tmpPoint.positionValue[2]-currentFollowPoint.positionValue[2]);
    currentFollowPoint=pointOut;
    if(moveRatio>=1)
    {
        pointQueue.dequeue();
    }

}


int FollowMotion:: initialSpeedRatioAxis()
{
    //轴初始化
    JointAttribute attribute;
    //trap用
    attribute.maxAccelation=6;
    //attribute.maxVelocity=1.7;
    attribute.maxVelocity=3;
    //spline用
//        attribute.maxAccelation=6;
//        attribute.maxVelocity=2;

    //trap用
    attribute.maxAccelation=3;
    //attribute.maxVelocity=1.7;
    attribute.maxVelocity=10;

    attribute.maxTorque=10000;
    attribute.maxPosition=1.1;
    attribute.minPosition=-0.1;
    attribute.coupleFlag=0;
    attribute.coupleJoint=0;
    attribute.coupleReducer=0;
    attribute.jointType=0;
    attribute.pulsePerRound=100;
    attribute.externAxisFlag=0;
    attribute.gearRatio=1;
    attribute.jointName="speedRatioAxis";
    attribute.mobile=false;
    attribute.processFollowError=2;
    attribute.screwPitch=1;
    attribute.targetFollowError=1;
    attribute.axisLogicalType=E_AXIS_LOGIC_TYPE_VIRTUAL_STAND_ALONE;

    speedRatioAxis=new AxisObject(1,attribute,motionMessage);
    speedRatioAxis->axisIdByRobot=1;
    speedRatioAxis->deltaTime=deltaTime;

    return 1;
}
